#include "KinematicObject.h"
#include "MathUtility.h"
#include "Timer.h"
#include "KinematicObjectManager.h"

namespace Engine
{
	KinematicObject::KinematicObject():m_maxSpeed(0.0f),
									   m_maxSpeedSquared(0.0f),
									   m_maxAngularAcc(0.0f),
									   m_maxAngularVel(0.0f),
									   m_maxAcc(0.0f),
									   m_rot(0.0f){}

	KinematicObject::KinematicObject(float maxSpeed, 
									 float maxAcc, 
									 float maxAngularAcc, 
									 float maxAngularVel):
									 m_maxSpeed(maxSpeed),
									 m_maxSpeedSquared(maxSpeed * maxSpeed),
									 m_maxAngularAcc(maxAngularAcc),
									 m_maxAngularVel(maxAngularVel),
									 m_maxAcc(maxAcc),
									 m_rot(0.0f){}

	void KinematicObject::Update()
	{
		if(m_steeringOutput.linear.SquaredLength() > m_maxSpeedSquared)
		{
			m_steeringOutput.linear.Normalize();
			m_steeringOutput.linear *= m_maxSpeed;
		}

		Vec3 oldVel = m_vel;
		m_vel = m_steeringOutput.linear;

		//update the position
		float dt = TheTimer::Instance() ->GetDT();
		m_pos += (oldVel + m_vel) * .05f * dt;
		m_rot += m_steeringOutput.angular * dt;

		m_steeringOutput.Reset();
	}

	void KinematicObject::Register()
	{
		TheKinematicObjectManager::Instance() ->RegisterKinematicObject(this);
	}

	void KinematicObject::Unregister()
	{
		TheKinematicObjectManager::Instance() ->UnregisterKinematicObject(GetGameObjectID());
	}

	KinematicObject::~KinematicObject()
	{
		Unregister();
	}
}